
// servos of turret
Servo servo_turret_low, servo_turret_high;

// IF Range sensors
const int infra_range_turret = 0;

// US Range sensors
const int sonic_range_turret = 8;

/////////////////
// Servos position
void reset_turret_position(const int dlay)
{
  current_turret_position_low = turret_low_mid;
  current_turret_position_high = turret_high_mid;
  servo_turret_low.write(turret_low_mid); 
  servo_turret_high.write(turret_high_mid);
  delay(dlay);    
}
void set_turret_position(const int low, const int high)
{
  int set_low = constrain(low, turret_low_min, turret_low_max);
  int set_high = constrain(high, turret_high_min, turret_high_max);

  // calcul delay from old position to new position
  int ml = abs(current_turret_position_low-set_low);
  int mh = abs(current_turret_position_high-set_high);
  long dlay = (ml+mh)*5;

  servo_turret_low.write(set_low); 
  servo_turret_high.write(set_high);
  current_turret_position_low = set_low;
  current_turret_position_high = set_high;

  delay(dlay);
}
void acc_turret_position(const int low, const int high)
{
  set_turret_position(current_turret_position_low + low, current_turret_position_high + high);
}
void setup_turret()
{
  servo_turret_low.attach(12);
  servo_turret_high.attach(13);
  reset_turret_position(50);
}

///////////////
// Range
short get_range_turret()
{
  switch(_beam_sensors)
  {
  case 0:
  case 1:
    {
      return get_range_sensor_gp2ya02(infra_range_turret, _beam_count, _beam_delay, _beam_precision, turret_ir_min, turret_ir_max);
    }
    break;
  case 2:
    {
      return get_range_sensor_ultrasonic(sonic_range_turret, _beam_count, _beam_delay, _beam_precision, turret_us_min, turret_us_max);
    }
    break;
  case (1+2):
    {
      short dist = get_range_sensor_gp2ya02(infra_range_turret, _beam_count, _beam_delay, _beam_precision, turret_ir_min, turret_ir_max);
      if(dist==-1) return get_range_sensor_ultrasonic(sonic_range_turret, _beam_count, _beam_delay, _beam_precision, turret_us_min, turret_us_max);
      else
      {
        if(dist >= turret_ir_max)
        {
          short dist2 = get_range_sensor_ultrasonic(sonic_range_turret, _beam_count, _beam_delay, _beam_precision, turret_us_min, turret_us_max);
          if(dist2 != -1 && dist2>dist) dist = dist2;
        }
        else if(dist <= turret_ir_min)
        {
          short dist2 = get_range_sensor_ultrasonic(sonic_range_turret, _beam_count, _beam_delay, _beam_precision, turret_us_min, turret_us_max);
          if(dist2 != -1 && dist2<dist) dist = dist2;
        }
      }
      return dist;
    }
    break;
  }
}



